import numpy as np
import ctypes
import cv2
from pykinect2 import PyKinectV2
from pykinect2.PyKinectV2 import *
from pykinect2 import PyKinectRuntime

kinect = PyKinectRuntime.PyKinectRuntime(PyKinectV2.FrameSourceTypes_Depth | PyKinectV2.FrameSourceTypes_Color)

""" import your images here """
color_path = "C:\\Users\\jiangshan\\Desktop\\Lock3DFace-demo\\color\\001_Kinect_FE_1COLOR\\01.jpg"
depth_path = "C:\\Users\\jiangshan\\Desktop\\Lock3DFace-demo\\depth\\001_Kinect_FE_1DEPTH\\01.png"

depth_img = cv2.imread(depth_path)
align_depth_img = cv2.imread(color_path)

color2depth_points_type = _DepthSpacePoint * np.int(1920 * 1080)
color2depth_points = ctypes.cast(color2depth_points_type(), ctypes.POINTER(_DepthSpacePoint))
kinect._mapper.MapColorFrameToDepthSpace(ctypes.c_uint(512 * 424), kinect._depth_frame_data, ctypes.c_uint(1920 * 1080), color2depth_points)
depthXYs = np.copy(np.ctypeslib.as_array(color2depth_points, shape=(kinect.color_frame_desc.Height*kinect.color_frame_desc.Width,)))
depthXYs = depthXYs.view(np.float32).reshape(depthXYs.shape + (-1,))
depthXYs += 0.5
depthXYs = depthXYs.reshape(kinect.color_frame_desc.Height, kinect.color_frame_desc.Width, 2).astype(np.int)
depthXs = np.clip(depthXYs[:, :, 0], 0, kinect.depth_frame_desc.Width - 1)
depthYs = np.clip(depthXYs[:, :, 1], 0, kinect.depth_frame_desc.Height - 1)
align_depth_img[:, :] = depth_img[depthYs, depthXs, :1]
print(align_depth_img.shape)
cv2.imshow('Aligned Image', cv2.resize(cv2.flip(align_depth_img, 1), (int(1920 / 2.0), int(1080 / 2.0))))
cv2.waitKey(0)